6.1 NMPC & ACADOS

0. 前言

本文是一篇面向初学者的 NMPC（非线性模型预测控制）入门教程，旨在帮助你建立从零到一的知识体系：理解什么是 NMPC、它解决什么问题、ACADOS 在其中扮演什么角色，以及如何用 ACADOS 的 Python 接口搭建一个可运行的 NMPC 控制器，并在 MuJoCo 中验证控制效果。

读完本文后，你将能够：

理解 MPC 和 NMPC 的核心思想及其数学表述
知道 ACADOS 是什么、为什么选择它
理解最优控制问题（OCP）是如何被构建和求解的
对一个完整的四旋翼 NMPC 控制实例有整体把握
知道如何调节关键参数来影响控制性能

2. 从 MPC 到 NMPC

2.1 什么是模型预测控制(MPC)

核心思想：在每个控制周期，利用系统的数学模型预测未来一段时间内的系统行为，然后求解一个优化问题来找到最优的控制序列，但只执行第一个控制量，下一周期重复整个过程。

这个思想可以用四个关键词概括：

关键词	含义
模型（Model）	使用数学方程描述系统的动态行为（如何从当前状态演变到未来状态）
预测（Predict）	在有限时域 $T_{f}$ 内，基于模型推算系统的未来状态轨迹
优化（Optimize）	在满足约束的前提下，求解使某个代价函数最小的控制序列
滚动时域（Receding Horizon）	只执行第一个控制量，下一周期用新的测量状态重新优化

2.2 为什么需要预测？

考虑一个日常类比：你开车时，眼睛看的不只是车头前方 1 米，而是几十米甚至更远。你根据前方的路况（弯道、障碍物）提前调整方向盘和油门，而不是等车轮压到弯道才开始打方向。这就是“预测 + 提前动作”的逻辑，MPC 正是将这种直觉数学化。

再类比一个徒步者的故事：他有一张局部地形图（这对应 MPC 的系统模型），能看到方圆一定范围内的地形（对应 MPC 的预测时域），但看不到远处的全貌。他的目标是走最短路径到达一个遥远的目的地。因为他只能看到有限范围内的地形，所以需要根据当前视野规划一段路线，走一段后，随着视野向前推移，他会根据新看到的信息重新规划。这与 MPC 的滚动时域优化思想完全一致：由于预测视野有限，未来的“障碍”（如约束）可能在当前时域内不可见，因此需要不断重新优化。

2.3 预测时域与控制时域

MPC 中有两个关键的时间概念：

概念	符号	含义
预测时域（Prediction Horizon）	$T_{p}$	控制器向前预测系统行为的时间长度
控制时域（Control Horizon）	$T_{c}$	控制器规划自由控制动作的时间长度

通常 $T_{c} \leq T_{p}$ ，即控制时域小于等于预测时域。在控制时域之后，控制量可以固定为常数（或由某个局部控制器决定），而系统状态的预测仍然持续到预测时域的末端。

在本教程的配套工程中，我们使用的配置使控制时域与预测时域相等：

T_{p} = T_{c} = N \cdot Δ t = 0.3 s

其中 $N$ 是打靶区间数（shooting intervals）， $Δ t$ 是每个区间的时间步长。通常来说，预测间隔时间 $Δ t$ 越短，预测时间 $T_{p}$ 越长，往往控制效果越好，但是区间数 $N$ 的增加会导致计算量的大幅度增加，可能无法实现实时计算。在有限的计算资源下，需要在控制效果和计算量之间达到尽可能的平衡。

2.4 NMPC 算法的基本步骤

将上述思想综合起来，NMPC 控制回路在每个采样时刻执行以下步骤：

获取状态：测量或估计系统的当前状态 $\hat{x} (t)$
求解 OCP：以 $\hat{x} (t)$ 为初始条件，求解一个在预测时域 $T_{p}$ 上的开环最优控制问题，得到最优控制序列 ${\bar{u}}^{*} (τ)$
执行第一步：将最优控制序列的第一个值 $u (t) = {\bar{u}}^{*} (t)$ 施加到系统上
滚动向前：等待到下一个采样时刻 $t + δ$ ，用新的测量状态回到步骤 1

这种“求解→执行第一步→重新求解”的循环被称为**滚动时域（Receding Horizon）**策略，是 MPC 区别于其他控制方法的最核心特征。

2.5 线性 MPC vs. 非线性 MPC

线性 MPC（Linear MPC）：使用线性模型 $x_{k + 1} = A x_{k} + B u_{k}$ 来描述系统。优点是优化问题为凸二次规划（QP），求解快速且保证全局最优，缺点是无法准确描述大多数真实系统的非线性特性。

非线性 MPC（NMPC）：使用非线性模型 $x_{k + 1} = f (x_{k}, u_{k})$ 来描述系统。优点是可以精确建模复杂的物理系统（如四旋翼的空气动力学、机械臂的关节动力学），缺点是优化问题为非凸非线性规划（NLP），求解更复杂且不一定能保证全局最优。

那么到底该选哪个？ 一个实用的判断标准：

如果系统在工作点附近近似线性（如巡航状态下的无人机），线性 MPC 可能足够。
如果系统大范围运动、动力学强烈耦合、或包含姿态变化，NMPC 是更好的选择。
如果实时性要求极高（kHz 级别）且算力有限，线性 MPC 更安全。现代 NMPC 求解器（如 ACADOS）已经可以在 100Hz-1kHz 下运行。

2.6 NMPC 的优势与代价

优势：

直接处理非线性动力学，不需要在工作点线性化
自然地处理状态和控制输入约束（如电机推力上下限）
可以显式优化多个目标（位置精度、姿态稳定性、能耗等）

代价：

计算量显著大于线性 MPC
求解器配置和参数调优更复杂
数值稳定性需要关注（病态的 Hessian、收敛问题等）

3. 最优控制问题（OCP）的数学表述

NMPC 的核心是一个在每个控制周期反复求解的最优控制问题（Optimal Control Problem, OCP）。理解 OCP 的数学结构是理解 NMPC 的基础。

3.1 OCP 的标准形式

一个离散时间的 OCP 通常表述为：

min_{x_{0}, \dots, x_{N}, u_{0}, \dots, u_{N - 1}} \sum_{k = 0}^{N - 1} \underset{过程代价}{\underset{⏟}{L (x_{k}, u_{k})}} + \underset{终端代价}{\underset{⏟}{E (x_{N})}}

满足约束：

\begin{aligned} x_{0} & = {\hat{x}}_{0} & (初始状态固定) \\ x_{k + 1} & = f (x_{k}, u_{k}) & (动力学约束) \\ u_{min} & \leq u_{k} \leq u_{max} & (控制输入约束) \\ x_{min} & \leq x_{k} \leq x_{max} & (状态约束，可选) \end{aligned}

其中各符号的含义：

符号	含义
$N$	预测时域的离散节点数（预测步数）
$T_{f} = N \cdot Δ t$	预测时域的实际时间长度
$x_{k} \in R^{n_{x}}$	第 $k$ 个节点处的系统状态向量
$u_{k} \in R^{n_{u}}$	第 $k$ 个区间上的控制输入向量
${\hat{x}}_{0}$	当前测量/估计得到的初始状态
$f (\cdot)$	系统动力学方程（离散化后）
$L (\cdot)$	过程代价函数（stage cost）
$E (\cdot)$	终端代价函数（terminal cost）

3.2 代价函数的设计

在本教程的工程代码中，代价函数采用**线性最小二乘（Linear Least Squares, LLS）**形式：

L (x_{k}, u_{k}) = ∥ V_{x} x_{k} - y_{ref, x} ∥_{Q}^{2} + ∥ V_{u} u_{k} - y_{ref, u} ∥_{R}^{2}

E (x_{N}) = ∥ V_{x}^{e} x_{N} - y_{ref}^{e} ∥_{Q_{e}}^{2}

其中：

$Q$ （ $W$ ）是 $17 \times 17$ 的过程代价权重矩阵（同时覆盖状态和控制）
$R$ 是控制输入权重（实际上在 acados 中合并进 $Q$ 矩阵，作为 $W$ 的对角块）
$Q_{e}$ （ $W_{e}$ ）是 $13 \times 13$ 的终端代价权重矩阵
$V_{x}, V_{u}$ 是映射矩阵，将状态和控制量映射到代价评估空间
$y_{ref}$ 是参考轨迹（期望的状态 + 期望的控制量）

注：这里的“线性”指的是代价函数中的残差 $y_{k} - y_{ref}$ 是状态和控制量的线性函数（由矩阵 $V_{x}, V_{u}$ 映射），而非指整个 OCP 是线性的。动力学约束 $x_{k + 1} = f (x_{k}, u_{k})$ 仍然是非线性的，因此这仍是一个非线性最优控制问题。在 NMPC 文献中，这种形式通常被称为**非线性最小二乘（NLS）**代价。

权重矩阵的作用：权重决定了不同目标之间的相对重要性。例如：

位置权重大 → 控制器优先保证位置精度，可以接受较大的姿态偏差
控制权重大 → 控制器更“保守”，优先减小控制量幅值
角速度权重小 → 允许更快的姿态机动

3.3 线性最小二乘代价的结构

在 acados 中使用 LINEAR_LS 代价类型时，代价评估函数的输出向量 $y$ 为：

y_{k} = [\begin{matrix} V_{x} \\ 0 \end{matrix}] x_{k} + [\begin{matrix} 0 \\ V_{u} \end{matrix}] u_{k}

其中 $V_{x} \in R^{n_{y} \times n_{x}}$ 将状态映射到代价空间， $V_{u} \in R^{n_{y} \times n_{u}}$ 将控制量映射到代价空间， $n_{y} = n_{x} + n_{u}$ 。

这样一个 17 维的 $y$ 向量就同时度量了状态偏离和控制偏离。

3.4 OCP 求解的两条路径：序贯法与联立法

将连续 OCP 离散化为有限维 NLP 后，有两种基本求解策略：

序贯法（Sequential Approach）：

只有控制变量 ${u_{0}, u_{1}, \dots, u_{N - 1}}$ 作为 NLP 的决策变量
在每次优化迭代中，通过数值积分“完整地”求解状态轨迹
状态在优化问题中不显式出现，而是由控制序列唯一确定
优点：NLP 维度小，实现简单
缺点：生成的 QP 矩阵是稠密的，无法利用 OCP 的稀疏结构，对初始猜测敏感

联立法（Simultaneous Approach）：

状态变量和控制变量同时作为 NLP 的决策变量
系统动力学以等式约束的形式（ $x_{k + 1} = f (x_{k}, u_{k})$ ）加入优化问题
优化收敛时，这些等式约束保证状态轨迹满足动力学
优点：生成的 QP 矩阵具有带状稀疏结构，可以利用这种结构大幅加速求解，相比单次打靶法，多重打靶法对初始猜测的敏感性更低，收敛区域更大
缺点：NLP 维度更大

ACADOS 采用联立法中的直接多重打靶法（Direct Multiple Shooting）：

将预测时域 $[0, T_{f}]$ 划分为 $N$ 个打靶区间（shooting intervals）
每个区间的起点状态 $s_{k}$ 作为决策变量引入 NLP
每个区间使用数值积分器从 $s_{k}$ 积分到 ${\tilde{s}}_{k + 1}$
在区间边界上施加连续性约束： $s_{k + 1} = {\tilde{s}}_{k + 1}$

多重打靶法结合了序贯法和联立法的优点：

带状稀疏结构：可以利用 OCP 的结构加速 QP 求解
并行化友好：每个区间的积分可以独立计算
数值稳定性好：对初始猜测不敏感，收敛性好于单次打靶法

注：在多重打靶法中，连续性约束 $s_{k + 1} = {\tilde{s}}_{k + 1}$ 是优化问题的约束之一。只有当优化完全收敛时，这些约束才被满足，状态轨迹才真正满足动力学。这与单次打靶法不同：在单次打靶法中，状态完全由控制序列通过积分唯一确定，因此任何控制序列都对应一条满足动力学的轨迹（但不一定满足其他约束）。

3.5 终端代价与稳定性

一个重要的问题是：使用有限预测时域 $T_{p}$ 的 NMPC 是否能够保证闭环系统稳定？直觉上，如果在每个采样时刻只优化未来一小段时间内的代价，系统可能会“短视”，导致不稳定。

为了保证稳定性，常用的方法是向 OCP 中添加终端代价（Terminal Cost）和终端约束（Terminal Constraint），将代价函数扩展为：

J = \sum_{k = 0}^{N - 1} L (x_{k}, u_{k}) + \underset{终端代价}{\underset{⏟}{E (x_{N})}}

并可选地要求：

x_{N} \in Ω (终端区域约束)

其中 $E (x)$ 和 $Ω$ 不是随意选择的：它们需要通过离线计算确定，使得在 $Ω$ 内存在一个局部控制器 $κ (x)$ 能够镇定系统，并且 $E (x)$ 是 $Ω$ 上的局部李雅普诺夫函数。当满足这些条件时，有限时域 NMPC 的闭环稳定性可以得到理论保证。

这种方法的直观理解：终端代价 $E (x_{N})$ 在理论上是对无限时域剩余代价的一个局部近似（通过离线求解系统在线性化点附近的 LQR 问题得到）。这种设计的目的是让有限时域 NMPC 在行为上尽可能接近无限时域 NMPC，从而保证闭环稳定性。在“准无限时域 NMPC”框架中（Chen & Allgöwer, 1998）， $E (x)$ 被设计为终端区域 $Ω$ 内的一个李雅普诺夫函数。

实践说明：在配套教程的工程实现中，我们只添加了终端代价 $W_{e} = Q$ （即直接将过程状态权重矩阵复制到终端），但并未添加终端区域约束 $x_{N} \in Ω$ 。这意味着，虽然我们在代价函数中鼓励状态在预测时域末端接近目标，但没有强制要求它落在某个特定区域内。从理论角度来看，这相当于放松了稳定性保证的条件，从工程角度来看，这种“启发式”方法在实践中通常能够工作，因为优化器会在代价函数的作用下自然地将终端状态推向目标附近，但它并不能从理论上严格保证闭环稳定性。如果需要对稳定性做严格保证，需要按准无限时域 NMPC 框架计算合适的终端代价矩阵和终端区域约束。

4. ACADOS 简介

4.1 ACADOS 是什么

acados（发音类似“A卡多斯”）是一个专为**最优控制问题（OCP）**设计的高性能求解器框架。它使用 C 语言编写核心算法，同时提供 Python、MATLAB、Octave 的高级接口。

acados 的核心定位可以总结为一句话：专门用于求解带有 OCP 结构的非线性规划（NLP），特别适合需要在嵌入式系统上实时运行的 MPC 和 MHE（滚动时域估计）应用。

4.2 为什么选择 ACADOS

在 NMPC 求解器的选择中，acados 有几个突出的优势：

1. 利用 OCP 结构加速求解

通用的 NLP 求解器（如 IPOPT）将问题视为一般的稀疏非线性规划，而 acados 深知底层的 OCP 结构（带状 Hessian、阶段间递推关系），可以利用这种结构大幅加速计算，速度可以比通用求解器快 10-100 倍。

2. 实时迭代方案（RTI）

acados 支持 SQP-RTI（Sequential Quadratic Programming - Real-Time Iteration）模式，每个控制周期只执行 1 次 SQP 迭代就返回控制量。这牺牲了单步的“最优性”，但保证了极低且可预测的延迟，非常适合高频控制回路。

3. 模块化架构

acados 允许用户像搭积木一样自由组合：

NLP 求解器（SQP、SQP-RTI、DDP）
QP 求解器（HPIPM、qpOASES、OSQP、Clarabel 等）
积分器（ERK 显式龙格-库塔、IRK 隐式龙格-库塔、GNSF 等）
全局化策略（固定步长、线搜索等）

4. 嵌入式部署

通过 Python/MATLAB 接口定义问题后，acados 可以生成独立的 C 代码，直接在嵌入式平台上编译运行，不需要 Python 运行时。

5. 灵敏度分析

acados 可以计算最优解对参数的灵敏度（导数），这对于将 MPC 与强化学习等机器学习方法结合非常重要。

4.3 ACADOS 的核心组件

┌──────────────────────────────────────────────────────┐
│                    AcadosOcp                         │
│  (问题定义: 模型、代价、约束、求解器选项)              │
└──────────────────┬───────────────────────────────────┘
                   │ 传入 json_file
                   ▼
┌──────────────────────────────────────────────────────┐
│               AcadosOcpSolver                        │
│  (C 代码生成/编译 + 求解接口)                         │
│  · solve()     → 执行优化                            │
│  · get()       → 获取状态/控制序列                   │
│  · set()       → 在线修改参考/约束                   │
│  · options_set() → 调整求解器行为                     │
└──────────────────┬───────────────────────────────────┘
                   │
                   ▼
┌──────────────────────────────────────────────────────┐
│              AcadosSim / AcadosSimSolver             │
│  (单独的系统仿真接口，用于验证模型和开环预测)           │
└──────────────────────────────────────────────────────┘

AcadosModel：用 CasADi 符号框架定义系统动力学。你提供非线性微分方程 f_expl（显式 ODE），acados 自动生成 Jacobian 和 Hessian 的 C 代码。

AcadosOcp：将所有配置（模型、维度、代价、约束、求解器选项）汇聚成一个 OCP 描述对象。

AcadosOcpSolver：接收 AcadosOcp 对象，生成并编译 C 代码，然后提供 solve() 方法来求解。

4.4 关键术语表

术语	说明
$N$ / $N_{horizon}$	预测时域的离散区间数（shooting intervals）
$t_{f}$	预测时域的总时间长度（秒）
`qp_solver`	处理 SQP 每次迭代中的 QP 子问题的求解器
`nlp_solver_type`	NLP 求解策略：`SQP`（全收敛）或 `SQP_RTI`（实时迭代）
`integrator_type`	时间离散化使用的数值积分方法
`hessian_approx`	Hessian 矩阵的近似方式：`GAUSS_NEWTON`（默认，适合最小二乘）或 `EXACT`（精确，需要更多计算）
`condensing`（压缩）	将多阶段 QP 问题降维的技术。全压缩（Full）内存小但计算开销大，部分压缩（Partial）平衡性好

5. ACADOS 如何求解 OCP

5.1 整体求解流程

acados 求解 OCP 的流程可以分为以下步骤：

1. 问题定义（Python/MATLAB）
   ├── 用 CasADi 符号定义系统模型 f(x, u)
   ├── 配置代价函数（权重矩阵、参考轨迹）
   ├── 配置约束（控制量上下界、状态初始值）
   └── 选择求解器选项

2. 代码生成（自动）
   ├── CasADi 将符号表达式编译为 C 代码
   ├── acados 生成 OCP 求解器的 C 框架代码
   └── C 编译器（gcc/clang）编译为动态库（.so/.pyd/.dll）
       Python 接口通过 ctypes 加载该动态库进行调用

3. 在线求解（每个控制周期）
   ├── set() 更新初始状态约束和参考轨迹
   ├── solve() 执行 SQP 迭代
   │   ├── 积分器前向传播（预测状态轨迹）
   │   ├── 计算 Jacobian / Hessian（灵敏度）
   │   ├── 构建并求解 QP 子问题（HPIPM 等）
   │   └── 更新解（SQP 步）
   └── get() 提取最优控制序列 u[0], u[1], ...

注：acados 默认使用 Python 的 subprocess 直接调用 C 编译器进行代码编译。用户也可以选择使用 CMake 来生成独立的可执行文件，但默认的 Python 接口流程是直接编译为动态库。

5.2 SQP（序列二次规划）工作原理

SQP 是求解 NLP 的核心方法。其思想是：既然 NLP 太难直接求解，那就反复用一个局部的二次规划（QP）来逼近它。

每一轮 SQP 迭代包含：

线性化：在当前点 $(x^{(i)}, u^{(i)})$ 将系统动力学一阶泰勒展开
二次近似：将代价函数在此点做二阶泰勒展开（或 Gauss-Newton 近似）
求解 QP：得到搜索方向 $(Δ x, Δ u)$
更新：沿搜索方向走一步（可能需要线搜索来保证收敛）
检查收敛：如果 KKT 条件残差足够小，则停止，否则回到步骤 1

非线性 NLP             →    近似为 QP  →  求解 QP  →  更新解

迭代直到收敛：  x^{(i+1)} = x^{(i)} + α · Δx
               u^{(i+1)} = u^{(i)} + α · Δu

5.3 SQP vs. SQP-RTI

特性	SQP	SQP-RTI
每周期 SQP 迭代次数	多次（直到收敛）	1 次
单次求解时间	较慢（可能需几 ms 到几十 ms）	极快（通常在亚毫秒级）
解的最优性	高（收敛到局部最优）	近似（“够用就好”）
适用场景	离线轨迹优化、仿真	实时控制、高频 MPC

在本配套项目中，使用 SQP_RTI 模式，并且设置 nlp_solver_max_iter = 1，即每个控制周期只做一次 SQP 迭代。这是 NMPC 在实时控制中的标准做法。

5.4 QP 求解器的选择

acados 支持 7 种 QP 求解器，以下是核心的几种：

QP 求解器	压缩策略	特点
`PARTIAL_CONDENSING_HPIPM`	部分压缩	默认推荐，速度快、数值稳定、支持热启动
`FULL_CONDENSING_HPIPM`	全压缩	内存占用小，适合嵌入式
`FULL_CONDENSING_QPOASES`	全压缩	经典活性集法，适合小规模问题
`PARTIAL_CONDENSING_OSQP`	部分压缩	基于 ADMM，适合大规模稀疏 QP

本配套项目使用 PARTIAL_CONDENSING_HPIPM（部分压缩 + HPIPM），这是 acados 的默认推荐选择，在速度、稳定性和内存占用之间取得了良好平衡。

5.5 热启动（Warm Start）

NMPC 的一个关键性能优化是热启动：上一个控制周期的解（状态轨迹和控制序列）通常是一个很好的初始猜测，将其作为当前周期的求解起点，可以大幅减少迭代次数。

acados 通过以下选项支持热启动：

qp_warm_start = 3：使用上一个 QP 解的内点信息来初始化当前 QP
warm_start_first_qp = True：第一次求解时也尝试热启动

6. 四旋翼NMPC控制ACADOS部署

配套目录下的参考工程使用 acados 实现了一个完整的四旋翼 NMPC 位置控制器。本节从概念层面介绍其组成。

6.1 系统概览

┌──────────┐    ┌──────────────┐    ┌──────────┐    ┌──────────┐
│ 目标位置  │───▶│ NMPC 控制器   │───▶│ 电机推力   │───▶│ MuJoCo   │
│ goal_pos │    │ (acados求解) │    │ [T1..T4] │    │ 四旋翼仿真│
└──────────┘    └──────┬───────┘    └──────────┘    └────┬─────┘
                       │                                │
                       │    ┌──────────────┐            │
                       └───◀│  状态预测器   │◀───────────┘
                            │ (RK4前向积分) │  传感器数据
                            └──────────────┘  (位置/速度/姿态)

数据流：每个控制周期（10ms），MuJoCo 仿真提供传感器数据（位置、速度、姿态四元数、角速度），控制器将其组装为 13 维状态量，通过 acados 求解最优控制序列，并执行第一个电机推力命令。

6.2 四旋翼动力学模型

状态变量（13 维）：

x = [p_{x}, p_{y}, p_{z}, v_{x}, v_{y}, v_{z}, q_{w}, q_{x}, q_{y}, q_{z}, ω_{x}, ω_{y}, ω_{z}]^{T}

分量	符号	描述
$p_{x}, p_{y}, p_{z}$	位置	世界坐标系下的三维位置（m）
$v_{x}, v_{y}, v_{z}$	速度	世界坐标系下的线速度（m/s）
$q_{w}, q_{x}, q_{y}, q_{z}$	四元数	姿态（从机体到世界的旋转）
$ω_{x}, ω_{y}, ω_{z}$	角速度	机体坐标系下的角速度（rad/s）

重要提示：四元数必须满足归一化约束 $∥ q ∥ = 1$ 。在实际实现中，状态预测和代价函数中的四元数分量都是在归一化约束下处理的。acados 本身不强制施加四元数归一化约束（除非用户显式添加该约束），因此如果使用自由四元数作为状态变量，需要在状态估计/预测环节保证其数值稳定性（例如在每次更新后显式归一化）。在配套工程中，状态预测器（predictor.py）在每次 RK4 积分后会执行四元数归一化操作。

控制输入（4 维）：

u = [T_{1}, T_{2}, T_{3}, T_{4}]^{T}

四个电机的推力（单位：N），每个电机的推力范围为 $[0, 1]$ N。

动力学核心方程：

线加速度（世界坐标系）：

\ddot{p} = \frac{1}{m} \cdot R_{w b} (q) \cdot [\begin{matrix} 0 \\ 0 \\ T_{1} + T_{2} + T_{3} + T_{4} \end{matrix}] - [\begin{matrix} 0 \\ 0 \\ g \end{matrix}]

其中 $R_{w b} (q)$ 是由四元数 $q$ 确定的机体→世界旋转矩阵， $m$ 是机体质量， $g$ 是重力加速度。

角加速度（机体坐标系，欧拉方程）：

\dot{ω} = I^{- 1} (τ - ω \times (I ω))

其中 $I = diag (I_{x x}, I_{y y}, I_{z z})$ 是转动惯量矩阵， $τ$ 是由四个电机推力产生的机体扭矩：

τ = [\begin{matrix} d (+ T_{1} - T_{2} - T_{3} + T_{4}) \\ d (- T_{1} - T_{2} + T_{3} + T_{4}) \\ k (- T_{1} + T_{2} - T_{3} + T_{4}) \end{matrix}]

其中 $d$ 是电机力臂长度， $k$ 是反扭矩系数。

重要：在配套代码中，上述动力学通过 CasADi 符号变量在 export_model.py 中定义，然后 acados 自动将其转为高效的 C 代码。

工程注记：在本教程的简化模型中，我们直接使用四个电机的推力 $T_{i}$ 作为控制输入。这种直接推力表述的优势是物理意义明确，且易于在优化问题中直接添加电机饱和约束 $0 \leq T_{i} \leq T_{max}$ 。在实际飞行控制器中，另一种常见做法是使用“总推力 + 三轴扭矩”作为控制量，然后通过控制分配矩阵转换为电机推力。两种方式维度相同（都是 4 个变量），各有优劣：直接推力方式使约束形式更直观，但优化变量之间的耦合通过动力学隐式表达，总推力+扭矩方式则使控制量的物理意义更清晰，且对姿态环的设计更直接。

6.3 代价函数设计

本工程的代价函数是一个线性最小二乘形式，目标是将四旋翼驱动到指定的目标状态。

过程代价权重（体现优化各个维度的优先级）：

状态分量	权重	物理含义
$p_{x}, p_{y}$	1.0	水平位置跟踪精度
$p_{z}$	2.0	高度跟踪精度（权重更高）
$v_{x}, v_{y}$	0.5	水平速度抑制
$v_{z}$	1.0	垂向速度抑制
$q_{w}$	0.01	姿态四元数实部
$q_{x}, q_{y}$	2.0	滚转/俯仰姿态（权重高 = 鼓励水平）
$q_{z}$	0.5	偏航姿态
$ω_{x}, ω_{y}$	0.001	角速度（允许机动）
$ω_{z}$	0.001	偏航角速度

注：四元数分量 $q_{x}, q_{y}$ 的权重高意味着“惩罚机体相对于水平面的倾斜”，因为四元数的水平分量 $q_{x}, q_{y}$ 直接与绕水平轴的旋转相关。在水平飞行时， $q_{x} \approx q_{y} \approx 0$ 。

控制代价：四个电机推力各有权重 4.0，防止控制量过大或振荡。

参考轨迹：

过程参考 y_ref：目标状态（期望的位置、零速度、水平姿态）+ 悬停推力
终端参考 y_ref_e：目标状态（无控制量部分）

6.4 求解器配置

本工程的关键求解器参数：

参数	设置值	说明
$t_{f}$	0.30 s	预测 300ms 的未来
$N$	30	30 个打靶区间，每步 10ms
`nlp_solver_type`	`SQP_RTI`	实时迭代模式
`nlp_solver_max_iter`	1	每周期只做 1 次 SQP 迭代
`qp_solver`	`PARTIAL_CONDENSING_HPIPM`	部分压缩 + 内点法 QP
`qp_solver_iter_max`	2	QP 子问题最多迭代 2 次
`integrator_type`	`ERK`	显式龙格-库塔积分器
`hessian_approx`	`GAUSS_NEWTON`	Gauss-Newton Hessian 近似

这些配置体现了“实时优先”的设计思路：牺牲单步的最优性来保证极快的求解速度，从而支持 100Hz 的控制频率。

6.5 关键设计：状态预测器

因为 NMPC 对控制环路中的延迟较为敏感，若 NMPC 预测的状态和实际的状态存在偏差，则会导致控制稳定性的大幅度下降，甚至控制发散。对于四旋翼控制系统来说，主要的延迟源自于电机延迟与求解延迟，对于常见的无刷电机，其延迟大概在 30~60ms 左右，即当前向电调发送的指令可能需要一段时间之后电机才能够通过加速或者减速达到设定的转速，这个延迟已经远大于 NMPC 的控制步之间的时间间隔，所以需要对延迟进行补偿。这里引入状态预测器（predictor.py）对延迟造成的影响进行预测和补偿：

利用系统的动力学模型（与 NMPC 使用相同的模型）
基于最近几个控制周期的电机推力历史
用 RK4（四阶龙格-库塔）进行开环前向积分
将预测状态作为 NMPC 求解的初始状态

这个设计弥补了求解延迟带来的影响，使控制器的表现更接近“零延迟”的理想情况。预测步数在 parameter.py 中通过 PREDICT_STEP = 3 设定（即提前 3 步预测，共约 30ms）。

7. 参数调优指南

NMPC 控制器有一系列参数需要调优。以下基于配套项目的实际参数，解释各参数的含义和调优方向。

7.1 预测时域参数（核心）

python

NMPC_TF_S = 0.30   # 预测时域长度 (s)
NMPC_N = 30        # 预测步数（shooting 节点数）

时域长度 tf：

太短（如 0.1s）：控制器“目光短浅”，容易产生激进的控制动作和超调
太长（如 2s）：计算量大，且远未来的预测不可靠（模型误差累积）
推荐范围：系统主要时间常数的 3-5 倍。四旋翼位置环的响应时间约 0.1-0.3s，所以 tf = 0.3s 是合理选择

步数 N：

步数越多 → 时间分辨率越高，但计算量线性增加
步数越少 → 求解快，但离散化误差大
推荐： $N$ 的选择应使每步时间 $d t = t_{f} / N$ 接近控制周期。本项目中控制周期是 10ms， $d t = 0.3 / 30 = 0.01 s = 10 ms$ ，刚好匹配

7.2 权重矩阵调优

python

NMPC_Q_POS_XY = 1.0      # ← 增大 → 更激进的位置跟踪
NMPC_Q_POS_Z  = 2.0      # ← 高度权重更高（安全优先）
NMPC_R_THRUST = 4.0      # ← 增大 → 更平滑的控制变化（注意：这是惩罚推力幅值，而非变化率）

关于控制权重的说明：配套工程中的 R 矩阵作用于控制量 $u$ 本身（幅值代价），目的是让控制器倾向于使用较小的推力。如果希望惩罚控制量的变化率（使控制更平滑），需要将代价作用于 $Δ u = u_{k} - u_{k - 1}$ 。acados 也支持这种形式（通过设置 cost.cost_type = "NONLINEAR_LS" 并自定义残差函数）。两者的效果不同：幅值代价 → 控制量整体偏小，变化率代价 → 控制量变化平缓。

状态权重 vs. 控制权重的权衡：

状态权重 $↑$ → 更积极跟踪目标，可能产生较大的控制动作
控制权重 $↑$ → 控制更保守（幅值更小），但跟踪精度下降

一个系统的调参方法：

首先将位置速度权重设置为 0，仅调节姿态权重和角速度权重，即调节姿态环
在姿态能够保持水平并能够在扰动下快速恢复的情况下，开始依次调节速度权重和位置权重，直到能够实现稳定的定点控制
如果不希望姿态波动太大，适当增大姿态权重
如果控制量变化过于剧烈（高频振荡），考虑引入控制变化率代价或增大现有控制权重
反复迭代，直到位置精度和姿态稳定性之间取得满意平衡

注意：权重是相对值。将所有权重同时乘以一个常数不会改变最优解（只是缩放代价函数）。

7.3 求解器参数调优

nlp_solver_type：

选项	适用场景
`SQP_RTI`	实时控制，需要确定的求解时间
`SQP`	离线优化，对解的最优性要求高

nlp_solver_max_iter：

1：标准 RTI 模式，仅做一次迭代
2-5：稍微多几步迭代，提高解质量，但增加延迟
增大此值可以改善大扰动后的恢复性能

qp_solver_iter_max：

QP 子问题的迭代上限
2 是实时控制中常用的折中选择
如果发现 QP 子问题不收敛，可以适当增加到 5-10

7.4 电机参数

python

MOTOR_TIME_CONSTANT = 0.031   # 电机一阶响应时间常数 (s)
MOTOR_MAX_THRUST = 1.0       # 单电机最大推力 (N)

电机时间常数：反映电机的响应速度。如果实际电机比仿真中更慢，需要增大此值以在控制器中补偿
推力上下限：确保约束不会太紧（导致 infeasibility）或太松（控制器可能输出物理上无法实现的推力）

7.5 常见问题与解决

症状	可能原因	解决方案
位置缓慢漂移，无法到达目标	位置权重太小	增大 `Q_POS_XY` / `Q_POS_Z`
姿态剧烈振荡	姿态权重太小，或控制权重太小	增大 `Q_QUAT_X/Y`，或增大 `R_THRUST`
求解时间过长	NLP/QP 迭代次数过多	减少 `nlp_solver_max_iter` 或 `qp_solver_iter_max`
求解器报 infeasible	约束太紧或初始状态太远	放宽控制约束，或减小 $t_{f}$ 避免远期不可行
稳态误差	模型与实际系统不匹配	检查质量、转动惯量等模型参数是否准确

8. NMPC 的稳定性与鲁棒性简介

在前面的章节中，我们主要关注了如何构建和求解 NMPC 问题。本节简要介绍 NMPC 的理论基础：稳定性与鲁棒性。这些内容有助于你理解求解器中的一些设计选择（如终端代价），但不需要深入掌握即可开始使用 NMPC。

8.1 为什么稳定性不是自动保证的

与 LQR 等经典最优控制方法不同，有限时域 NMPC 的稳定性并非自动成立。原因在于：

预测 ≠ 实际：即使在标称（无扰动、模型精确）情况下，开环预测轨迹与实际闭环轨迹也是不同的。这是因为每个采样时刻都在滚动优化，在 $t$ 时刻规划的后续控制量 $u_{1}, u_{2}, \dots$ 在 $t + δ$ 时刻会被重新规划。
“短视”问题：如果预测时域太短，控制器只关心近期的代价，可能做出导致远期不稳定但近期代价小的决策。

因此，不能简单地将期望的性能目标函数直接用作代价函数，通常需要修改 NMPC 的设置来保证闭环稳定性。

8.2 保证稳定性的主要方法

方法一：无限时域 NMPC

最直接的方法是使用无限预测时域，即 $T_{p} = \infty$ 。在标称情况下，此时最优代价函数（值函数 $V (x)$ ）可以作为闭环系统的李雅普诺夫函数，从而保证渐近稳定。然而这在计算上是不可行的，因为无法实时求解无限维的优化问题。

方法二：零终端约束

在预测时域末端强制 $x_{N} = 0$ （或目标稳态）。概念简单，但往往过于保守：系统必须在有限时间内被驱动到原点，导致可行域（吸引域）较小。

方法三：终端代价 + 终端区域约束（准无限时域 NMPC）

这是目前最常用且实用的一类方法。在第 3.5 节中已介绍了其基本思想：

在代价函数末尾添加终端代价 $E (x_{N})$
添加终端区域约束 $x_{N} \in Ω$
$E$ 和 $Ω$ 通过离线计算确定（通常基于系统在原点的线性化 $A = \partial f / \partial x, B = \partial f / \partial u$ ，求解李雅普诺夫方程得到终端代价矩阵 $P$ ）

这种方法的优势在于：即使使用较短的预测时域，也能保证闭环稳定。它与方法二（零终端约束）的关键区别在于： $Ω$ 是一个区域/集合（状态可以落在其中任意位置），而零终端约束要求状态精确等于原点，前者具有更大的可行域，保守性更低。

工程提醒：在配套教程的实现中，我们只添加了终端代价但没有添加终端区域约束。因此，从严格的理论角度看，我们的控制器并未充分满足准无限时域 NMPC 的稳定性条件。在实践中，这通常不影响仿真运行，但如果需要将控制器部署到真实硬件上，建议按理论要求添加终端约束。

方法四：收缩约束

要求预测状态轨迹在时域末端比初始状态更接近原点： $∥ x_{N} ∥ \leq α ∥ x_{0} ∥$ ，其中 $0 < α < 1$ 。

8.3 鲁棒性：固有与显式

在实际应用中，模型与真实系统之间必然存在偏差（模型失配、外部扰动）。鲁棒性处理这一问题。

固有鲁棒性（Inherent Robustness）：

NMPC 的滚动时域反馈机制本身就提供了一定的鲁棒性：通过在每个采样时刻用实际测量状态重新初始化，反馈可以补偿一定程度的模型误差。对某些特定类型的系统（如输入仿射系统、扇形有界不确定性），NMPC 的固有鲁棒性可以得到理论证明（通过“逆最优性”概念）。

鲁棒 NMPC 方案：

对于更大的不确定性，需要专门设计鲁棒 NMPC 方案，例如：

Min-Max NMPC：最小化最坏情况下的代价（在所有可能的不确定性下）
Tube NMPC：使用“管道”概念保证状态轨迹保持在一个有界管道内
输出反馈 NMPC：将状态估计器（如高增益观测器、MHE）与 NMPC 结合，处理部分状态不可测的情况

8.4 实践建议

对于入门应用，不需要深入实现鲁棒 NMPC 方案。以下是一些实用建议：

选择合适的预测时域： $T_{p}$ 应足够长以覆盖系统的重要动态。经验法则是 $T_{p}$ 至少为系统主要时间常数的 3-5 倍。如果发现闭环不稳定，首先尝试增大 $T_{p}$
使用终端代价：即使不严格计算终端代价矩阵，在终端施加与过程相同（或更大）的权重也有助于改善稳定性
约束不要过紧：过紧的状态/控制约束会导致优化不可行，进而导致控制器失效
利用热启动：acados 的热启动功能可以在优化问题遇到困难时提供接近前一步的解，减缓数值问题

本章参考资料与引申阅读已汇总至参考资料。

6.1 NMPC & ACADOS ​

0. 前言 ​

目录 ​

2. 从 MPC 到 NMPC ​

2.1 什么是模型预测控制(MPC) ​

2.2 为什么需要预测？ ​

2.3 预测时域与控制时域 ​

2.4 NMPC 算法的基本步骤 ​

2.5 线性 MPC vs. 非线性 MPC ​

2.6 NMPC 的优势与代价 ​

3. 最优控制问题（OCP）的数学表述 ​

3.1 OCP 的标准形式 ​

3.2 代价函数的设计 ​

3.3 线性最小二乘代价的结构 ​

3.4 OCP 求解的两条路径：序贯法与联立法 ​

3.5 终端代价与稳定性 ​

4. ACADOS 简介 ​

4.1 ACADOS 是什么 ​

4.2 为什么选择 ACADOS ​

4.3 ACADOS 的核心组件 ​

4.4 关键术语表 ​

5. ACADOS 如何求解 OCP ​

5.1 整体求解流程 ​

5.2 SQP（序列二次规划）工作原理 ​

5.3 SQP vs. SQP-RTI ​

5.4 QP 求解器的选择 ​

5.5 热启动（Warm Start） ​

6. 四旋翼NMPC控制ACADOS部署 ​

6.1 系统概览 ​

6.2 四旋翼动力学模型 ​

6.3 代价函数设计 ​

6.4 求解器配置 ​

6.5 关键设计：状态预测器 ​

7. 参数调优指南 ​

7.1 预测时域参数（核心） ​

7.2 权重矩阵调优 ​

7.3 求解器参数调优 ​

7.4 电机参数 ​

7.5 常见问题与解决 ​

8. NMPC 的稳定性与鲁棒性简介 ​

8.1 为什么稳定性不是自动保证的 ​

8.2 保证稳定性的主要方法 ​

8.3 鲁棒性：固有与显式 ​

8.4 实践建议 ​

6.1 NMPC & ACADOS

0. 前言

目录

2. 从 MPC 到 NMPC

2.1 什么是模型预测控制(MPC)

2.2 为什么需要预测？

2.3 预测时域与控制时域

2.4 NMPC 算法的基本步骤

2.5 线性 MPC vs. 非线性 MPC

2.6 NMPC 的优势与代价

3. 最优控制问题（OCP）的数学表述

3.1 OCP 的标准形式

3.2 代价函数的设计

3.3 线性最小二乘代价的结构

3.4 OCP 求解的两条路径：序贯法与联立法

3.5 终端代价与稳定性

4. ACADOS 简介

4.1 ACADOS 是什么

4.2 为什么选择 ACADOS

4.3 ACADOS 的核心组件

4.4 关键术语表

5. ACADOS 如何求解 OCP

5.1 整体求解流程

5.2 SQP（序列二次规划）工作原理

5.3 SQP vs. SQP-RTI

5.4 QP 求解器的选择

5.5 热启动（Warm Start）

6. 四旋翼NMPC控制ACADOS部署

6.1 系统概览

6.2 四旋翼动力学模型

6.3 代价函数设计

6.4 求解器配置

6.5 关键设计：状态预测器

7. 参数调优指南

7.1 预测时域参数（核心）

7.2 权重矩阵调优

7.3 求解器参数调优

7.4 电机参数

7.5 常见问题与解决

8. NMPC 的稳定性与鲁棒性简介

8.1 为什么稳定性不是自动保证的

8.2 保证稳定性的主要方法

8.3 鲁棒性：固有与显式

8.4 实践建议